package mybox.engine.linefollower;

import java.awt.Polygon;
import java.io.IOException;
import java.util.ArrayList;
import static java.lang.Math.*;

public class Carline {
	
	double deltaS=10;
	double alphaDeg=10;
	double alpha=toRadians(alphaDeg);
	public static double carLengtha=100;
	double psiRad;
	double psiDeg;
	public ArrayList<Polygon> carposList = new ArrayList<Polygon>();
	
	double hinterePosx=0;
	double hinterePosy=0;
	double initFahrzeugWinkel=PI/2;
	double initAlpha=toRadians(30);
	double initDeltaS=10;

	
	Carstate initCarState =null;
		
	
	Carstate currState=initCarState;
	
	public Carline(ArrayList<Polygon> course, int intFactor, double initFahrzeugWinkelDeg, double initAlphaDeg) throws SecurityException, IOException {
		
		initFahrzeugWinkel=toRadians(initFahrzeugWinkelDeg);
		hinterePosx=course.get(0).xpoints[0]/intFactor-carLengtha*cos(initFahrzeugWinkel);
		hinterePosy=course.get(0).ypoints[0]/intFactor-carLengtha*sin(initFahrzeugWinkel);
		initAlpha=toRadians(initAlphaDeg);
		initCarState = 
			initState(
				hinterePosx,
				hinterePosy,
				initFahrzeugWinkel,			
				initAlpha,
				initDeltaS
				);
		currState=initCarState;
		for (int polyIdx=0;polyIdx<100;polyIdx++) {
			carposList.add(currState.getPolygon(intFactor));
			currState=currState.nextCarState(currState);
		}		
		
	}
	
	public Carstate initState(
			double hinterePosx,
			double hinterePosy,
			double initFahrzeugWinkel,
			double initAlpha,
			double initDeltaS
			
	) {		
		Carstate retState= 
			new Carstate(
					hinterePosx,
					hinterePosy,
					initFahrzeugWinkel,
					hinterePosx+carLengtha*cos(initFahrzeugWinkel),
					hinterePosy+carLengtha*sin(initFahrzeugWinkel),
					initAlpha,
					initDeltaS
					);		
		return retState;
		
	}
	
	

}
